#!/usr/bin/env python

import rospy, os, sys
import actionlib
import roslib;
import rospy
from lsr import listener
from std_msgs.msg import String
from my_sound import Sound
 
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from manipulation import Manipulation  # 导入Manipulation类

 
waypoints = [  # <1>
    [(8.000,1.000,0.000), (0.000,0.000,-0.548,0.836)],
    [(12.000,-1.500,0.000), (0.000,0.000,-0.548,0.836)],
]

def goal_pose(pose):  # <2>
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]
 
    return goal_pose

def callback(msg):
        #print msg.data
        pub2 = rospy.Publisher('voiceWords',String,queue_size = 1)
        if msg.data.find('meet') > -1:
            pose = waypoints[0]
            sound = Sound()
            sound.say('hello, I am a clever robot,nice to meet you', 2)
  
        elif msg.data.find('help') > -1:
            pose = waypoints[1]
            sound = Sound()
            sound.say('sure,it is my pleasure', 2)
        
        elif msg.data.find('follow') > -1:
            pose = waypoints[0]
            sound = Sound()
            sound.say('ok, I will follow you', 2)

        elif msg.data.find('stop') > -1:
            sound = Sound()
            # sound.say('ok, I will stop', 2)

        elif msg.data.find('give') > -1:
            sound = Sound()
        

        else:
            return
        


        client = actionlib.SimpleActionClient('move_base', MoveBaseAction)  # <3>
        client.wait_for_server()
            
        
        if msg.data.find('meet') > -1:
            print("goal:x=%f y=%f"%(pose[0][0],pose[0][1]))
            goal = goal_pose(pose)
            client.send_goal(goal)
            client.wait_for_result()
            listen = listener()
            sound = Sound()
            order = -1
            rate = rospy.Rate(10)
            while True:
                order = listen.meet()
                rospy.sleep(1)
                if order == 0:
                    break
                else:
                    continue          
            sound.say('I have arrived, my master', 2)
            listen.help() 

        elif msg.data.find('help') > -1:
            print("goal:x=%f y=%f"%(pose[0][0],pose[0][1]))
            goal = goal_pose(pose)
            client.send_goal(goal)
            client.wait_for_result()
            listen1 = listener()
            order = 4  
            rate = rospy.Rate(10)
            while True:
                order = listen1.meet()
                rospy.sleep(1)
                if order == 5:
                    break
                else:
                    continue 
            sound = Sound()
            listen1.help()
            sound.say('I have arrived, my master', 2)
        
        elif msg.data.find('follow') > -1:
            listen2 = listener()
            order = 1  
            rate = rospy.Rate(10)
            while True:
                order = listen2.meet()
                rospy.sleep(1)
                if order == 2:
                    break
                else:
                    continue 
            sound = Sound()
            listen2.help()
            # sound.say('ok, I will follow you', 2)

        elif msg.data.find('stop') > -1:
            listen2 = listener()
            order = 2  
            rate = rospy.Rate(10)
            while True:
                order = listen2.meet()
                rospy.sleep(1)
                if order == 3:
                    break
                else:
                    continue 
            sound = Sound()
            listen2.help()
            sound.say('ok, I will stop', 2)

        elif msg.data.find('give') > -1:
            listen2 = listener()
            order = 5  
            rate = rospy.Rate(10)
            while True:
                order = listen2.meet()
                rospy.sleep(1)
                if order == 6:
                    break
                else:
                    continue 
            sound = Sound()
            listen2.help()
            sound.say('ok, here you are', 2)
        


        


 
if __name__ == '__main__':
    rospy.init_node('message_publisher')


    pub=rospy.Publisher('voiceWakeup',String,queue_size = 1)

    rospy.sleep(1)
    pub.publish('wake up')

    sub = rospy.Subscriber('voiceWords', String,callback)

    rospy.spin()
